/** \file   serialprintf.c
 *
 *  \brief  This file contains
 *
 *   This file performs functions on the following:
 *
 *      1. UNDEFINED.
 *
 *   Note: N/A
 *
 *  \author     Jiawei Xie
 *
 *  \version    0.1   Created newly for NAVCON
 */


#include "funcdef.h"

/*
 * External references for serial_printf
 */



//system time stamp
extern short TIME_STAMP_H;
extern short TIME_STAMP_L;
/*
 * Starting message printing string
 * Note: Buffer alignement is required only when working in DMA Mode.
 */
#define SERIAL_BUFFER_SIZE 		256
#pragma DATA_ALIGN(serial_buffer, SERIAL_BUFFER_SIZE);
char serial_buffer[SERIAL_BUFFER_SIZE];
/* Tx memory buffer */
#pragma DATA_ALIGN(Uart_TxBuffer, SERIAL_BUFFER_SIZE);
static Int8  Uart_TxBuffer[SERIAL_BUFFER_SIZE];

int DATA_LOG_TYPE = TXT_DATA_LOG;
//Int16 short
static short bin_serial_buf[SERIAL_BUFFER_SIZE];

int start_index,end_index,remaining = 0;
int txbytes = 0;
int dec_count = 0;
/* ========================================================================== */
/*                        FUNCTION DEFINTIONS                                 */
/* ========================================================================== */

void process_buffer(TSK_Ctrl_b * tsk, int type);
void serial_output( char* format, ... );
void serial_start_sending_data(TSK_Ctrl_b * tsk, char* buf, unsigned int len);
void endian_swap(void);

/*
 * NOTE: serial print maximum run at 45.5Hz with 83 char payload.
 * transfer maximum ~ 30212 bit/second
 * around 33% if using 115200 baud rate (11520 bytes)
 */
void datalog_tsk(void)
{

	TSK_Ctrl_b *self = &(SCB.TCB[TSK_datalog]);

	for(;;){
		SEM_pend(&datalog_sem, SYS_FOREVER);
		process_buffer(self,DATA_LOG_TYPE);
		TSK_yield();
	}
}


void process_buffer(TSK_Ctrl_b * tsk, int type){

	int i = 0;
	int nchars = 0;

	char check_sum = 0;
	char temp[2] = {0};

	if(type == TXT_DATA_LOG){	//send raw short data to com

		nchars = snprintf (serial_buffer, SERIAL_BUFFER_SIZE, "%x:%x:%x:%x:%x:%x:%x:%x:%x:%x:%x",
				vNavData.rGyro_x,vNavData.rGyro_y,vNavData.rGyro_z,vNavData.rGyro_T,
				vNavData.rBMA180_x,vNavData.rBMA180_y,vNavData.rBMA180_z,vNavData.rBMA180_T,
				vNavData.rMag_x,vNavData.rMag_y,vNavData.rMag_z);

		for(i=0;i<nchars-1;i++){
			check_sum += serial_buffer[i];
		}
		serial_buffer[nchars-1] = '\r';
		serial_buffer[nchars] = '\n';
		serial_buffer[nchars+1] = '\0';
		txbytes = strlen(serial_buffer);
		serial_start_sending_data(tsk, (char*)&serial_buffer, txbytes);
		//LOG_printf(tsk->trace, "TXT_DATA_LOG datalog size %d", txbytes);
	}else if (type == BIN_DATA_LOG){	//send bin data to com
		bin_serial_buf[0] = 0xFF81;
		bin_serial_buf[1] = SCB.TIME_STAMP_H;
		bin_serial_buf[2] = SCB.TIME_STAMP_L;
		bin_serial_buf[3] = vNavData.rGyro_x;
		bin_serial_buf[4] = vNavData.rGyro_y;
		bin_serial_buf[5] = vNavData.rGyro_z;
		bin_serial_buf[6] = vNavData.rGyro_T;
		bin_serial_buf[7] = vNavData.rBMA180_x;
		bin_serial_buf[8] = vNavData.rBMA180_y;
		bin_serial_buf[9] = vNavData.rBMA180_z;
		bin_serial_buf[10] = vNavData.rBMA180_T;
		bin_serial_buf[11] = vNavData.rMag_x;
		bin_serial_buf[12] = vNavData.rMag_y;
		bin_serial_buf[13] = vNavData.rMag_z;
		bin_serial_buf[14] = (ACC_CURRENT_SCALE_INDEX & 0xF) << 4 | (MAG_CURRENT_SCALE_INDEX & 0xF);
		for(i=1;i<15;i++){
			temp[0] = bin_serial_buf[i] >> 8;
			temp[1] = bin_serial_buf[i] & 0xFF;
			check_sum += temp[0];
			check_sum += temp[1];
		}
		bin_serial_buf[14] = bin_serial_buf[14] << 8 | check_sum;

		endian_swap();
		txbytes = 30; //15 shorts
		serial_start_sending_data(tsk, (char*)&bin_serial_buf, txbytes);

		////LOG_printf(tsk->trace, "BIN_DATA_LOG datalog size %d",txbytes);

	}else if(type == FULL_SET_SCALE_DATA_LOG){		//send txt double data to com
		txbytes = 30;
		//LOG_printf(&trace, "nchars = %d, strlen = %d",nchars,strlen(serial_buffer));
		//remaining = strlen(serial_buffer);
		//start_index = 0;
		//serial_start_sending_data(tsk, (char*)&serial_buffer, strlen(serial_buffer));
		if(remaining > txbytes){
			serial_start_sending_data(tsk,(char*)&serial_buffer[start_index], txbytes);
			remaining = remaining - txbytes;
			start_index = start_index + txbytes;
			//LOG_printf(&trace, "2: start_index %d : remaining %d",start_index,remaining);
		}else if(remaining == 0){
			bzeros((unsigned char*)serial_buffer,SERIAL_BUFFER_SIZE);
			nchars = snprintf (serial_buffer, SERIAL_BUFFER_SIZE,
					"Gx%.3f Gy%.3f Gz%.3f Gt%.3f Ax%.3f Ay%.3f Az%.3f At%.3f Mx%.3f My%.3f Mz%.3f Bp%.3f Bt%.3f",
					vNavData.Gyro_x,vNavData.Gyro_y,vNavData.Gyro_z,vNavData.Gyro_T,
					vNavData.BMA180_x,vNavData.BMA180_y,vNavData.BMA180_z,vNavData.BMA180_T,
					vNavData.Mag_x,vNavData.Mag_y,vNavData.Mag_z,
					vNavData.P,vNavData.T);
			for(i=0;i<GPS_INFO_LEN;i++)				//add from here
				serial_buffer[nchars-1+i] = vNavData.gps_data.data[i];
			
			serial_buffer[nchars-1+GPS_INFO_LEN] = '\r';
			serial_buffer[nchars+GPS_INFO_LEN] = '\n';
			serial_buffer[nchars+GPS_INFO_LEN+1] = '\0';		//add end here
			//serial_buffer[nchars-1] = '\r';
			//serial_buffer[nchars] = '\n';
			//serial_buffer[nchars+1] = '\0';
			//update start_index and remaining bytes
			start_index = 0;
			remaining = strlen(serial_buffer);
			//LOG_printf(tsk->trace, "FULL_SET_SCALE_DATA_LOG datalog size %d",remaining);
			//start transmit the first txbytes
			serial_start_sending_data(tsk,(char*)&serial_buffer[start_index], txbytes);
			remaining = remaining - txbytes;
			start_index = start_index + txbytes;
			//LOG_printf(&trace, "1: start_index %d : remaining %d",start_index,remaining);
		}else{
			//txbytes > remaining > 0
			serial_start_sending_data(tsk,(char*)&serial_buffer[start_index], remaining);
			//LOG_printf(&trace, "3: start_index %d : remaining %d",start_index,remaining);
			remaining = 0;
		}

	}else if(type == SYSTEM_PROFILING){
		if(dec_count == 50){
			//run at 50Hz/50 = 1Hz
			nchars = snprintf (serial_buffer, SERIAL_BUFFER_SIZE,
					"TSK_BMA180 %.3f\r\nTSK_ITG3205 %.3f\r\nTSK_HMC5883L %.3f\r\nTSK_MS5611 %.3f\r\nTSK_GPS %.3f\r\nMATRIX_TEST %.3f",
					SCB.TCB[TSK_BMA180].avg_t,SCB.TCB[TSK_ITG3205].avg_t,SCB.TCB[TSK_HMC5883L].avg_t,
					SCB.TCB[TSK_MS5611].avg_t,SCB.TCB[TSK_GPS].avg_t,SCB.TCB[MATRIX_TEST].avg_t);

			serial_buffer[nchars-1] = '\r';
			serial_buffer[nchars] = '\n';
			serial_buffer[nchars+1] = '\0';
			txbytes = strlen(serial_buffer);
			serial_start_sending_data(tsk, (char*)&serial_buffer, txbytes);
			dec_count = 0;
		}else{
			dec_count++;
		}

	}
}



void endian_swap(void)
{
	int i = 0;
	short temp = 0;
	for(i=0; i<(txbytes/2);i++){
		temp = (bin_serial_buf[i] >> 8) & 0x00FF;
		bin_serial_buf[i] = temp|(bin_serial_buf[i]<<8);
	}
}

// add this text to the output buffer
void serial_output( char* format, ... )
{
	va_list arglist ;
	va_start(arglist, format) ;

	int start_index = end_index ;
	int remaining = SERIAL_BUFFER_SIZE - start_index ;

	if (remaining > 1)
	{
		int wrote = vsnprintf( (char*)(&serial_buffer[start_index]), (size_t)remaining, format, arglist) ;
		end_index = start_index + wrote;
	}

	va_end(arglist);

	return ;
}

//start UART transaction
void serial_start_sending_data(TSK_Ctrl_b *tsk, char* buf, unsigned int len)
{
    Int     status = 0;
    memcpy(Uart_TxBuffer,buf,len);

	if(SEM_pendBinary(&UART2_OUT_SEM,SYS_TIME_OUT)){

		status = GIO_submit(tsk->outhd,IOM_WRITE, Uart_TxBuffer, &len, NULL);
		SEM_postBinary(&UART2_OUT_SEM);

	}else{
		//time out
		LOG_printf(tsk->trace, "UART2 Timeout!\n");
		status = -1;
	}

    if(!((status == IOM_COMPLETED)))
    {
        LOG_printf(tsk->trace, "\r\n Error from serial_start_sending_data Code %d\n",status);
    }
}

/* ========================================================================== */
/*                              END OF FILE                                   */
/* ========================================================================== */
